﻿using System.Collections.Generic;
using UnityEngine;

public static class MapEx
{
    public static Vector3 IgnoreY(this Vector3 vector)
    {
        return new Vector3(vector.x, 0, vector.z);
    }
}

public class Map
{
    public enum BorderClockWise
    {
        None,
        ClockWise,
        CounterClockWise,
    }

    public readonly List<ObstacleBlock> Blocks = new();

    public Vector3[] Border;

    public BorderClockWise BorderOrient = BorderClockWise.None;

    public GameObject GoSafeRoot;

    public void Clear()
    {
        Blocks.Clear();
        Border = null;
        GoSafeRoot = null;
    }

    public void Add(ObstacleBlock block)
    {
        for (int i = 0; i < 4; i++)
        {
            var v = block.PathPoints[i];

            if (Core.Utils.Math.IsInPolygon(Border, v.Value) == false)
            {
                v.Disable = true;
            }
        }

        foreach (var item in Blocks)
        {
            if (Core.Utils.Intersect.IsIntersectRectWithRect(item.Points, block.Points))
            {
                for (int i = 0; i < 4; i++)
                {
                    var v = item.PathPoints[i];

                    if (Core.Utils.Math.IsInPolygon(block.Points, v.Value))
                    {
                        v.Disable = true;
                    }

                    var v1 = block.PathPoints[i];
                    if (Core.Utils.Math.IsInPolygon(item.Points, v1.Value))
                    {
                        v1.Disable = true;
                    }
                }
            }
        }

        Blocks.Add(block);
    }

    public void Remove(ObstacleBlock block)
    {
        Blocks.Remove(block);
    }

    /// 获取距离Pos最近的可行走点，暂不考虑多个Block重叠的情况
    /// <summary>
    /// <param name="pos"></param>
    /// <returns></returns>
    /// </summary>
    public Vector3 GetNearest(Vector3 pos)
    {
        if (Border != null && Border.Length != 0)
        {
            UpdatePosWhenOutsideBorder(ref pos);
        }

        //m_temp_lst.Clear();
        ObstacleBlock block = null;

        foreach (var item in Blocks)
        {
            var dis = Vector3.Distance(pos, item.Center);
            if (dis > item.Length)
            {
                continue;
            }

            if (Core.Utils.Math.IsInPolygon(item.Points, pos))
            {
                //m_temp_lst.Add(item);
                block = item;
                break;
            }
        }

        if (block == null)
        {
            return pos;
        }

        float minDis = float.MaxValue;
        int idx = 0;
        for (int i = 0; i < block.Points.Length; i++)
        {
            var item = block.Points[i];
            var dis = Vector3.Distance(pos, item);
            if (dis < minDis)
            {
                minDis = dis;
                idx = i;
            }
        }

        var cnt = block.Points.Length;
        var pLeft = block.Points[(idx - 1 + cnt) % cnt];
        var pCenter = block.Points[idx];
        var pRight = block.Points[(idx + 1) % cnt];

        var dis1 = Core.Utils.Math.GetDisPointToLine(pos, pLeft, pCenter);
        var dis2 = Core.Utils.Math.GetDisPointToLine(pos, pRight, pCenter);

        Vector3 safePos;

        if (dis1 <= dis2)
        {
            var dir = (pCenter - pRight).normalized;
            safePos = pos + dir * (dis1 + 0.1f);
            if (!IsWalkable(safePos))
            {
                dir = (pCenter - pLeft).normalized;
                safePos = pos + dir * (dis2 + 0.1f);
            }
        }
        else
        {
            var dir = (pCenter - pLeft).normalized;
            safePos = pos + dir * (dis2 + 0.1f);
            if (!IsWalkable(safePos))
            {
                dir = (pCenter - pLeft).normalized;
                safePos = pos + dir * (dis2 + 0.1f);
            }
        }

        return safePos;
    }

    /// <summary>
    /// 野外场景根据行动路径获取最近的终点
    /// </summary>
    /// <param name="startPos">行动起始点</param>
    /// <param name="dir">行动方向</param>
    /// <returns></returns>
    public Vector3 GetNearest(Vector3 startPos, Vector3 dir)
    {
        if (dir.magnitude < 0.2f)
        {
            Debug.LogError("AStar.GetNearest -> 方向太模糊");
            return startPos;
        }

        dir.y = 0;
        dir = dir.normalized;
        int[] nearestBorderIdx = new int[] { -2, -1, 0, 1, 2 };
        int maxCosIdx = 0;
        float maxCos = float.MinValue;
        for (int i = 0; i < Border.Length; ++i)
        {
            Vector3 ori2Border = (Border[i] - startPos);
            ori2Border.y = 0;
            float cos = Vector3.Dot(ori2Border, dir);
            if (cos < 0)
            {
                continue;
            }

            cos /= ori2Border.magnitude;
            if (cos > maxCos)
            {
                maxCos = cos;
                maxCosIdx = i;
            }
        }

        for (int i = 0; i < nearestBorderIdx.Length; ++i)
        {
            nearestBorderIdx[i] += maxCosIdx;
            if (nearestBorderIdx[i] < 0)
            {
                nearestBorderIdx[i] += Border.Length;
            }

            if (nearestBorderIdx[i] >= Border.Length)
            {
                nearestBorderIdx[i] -= Border.Length;
            }
        }

        var endPos = startPos + dir * 100;
        for (int i = 0; i < nearestBorderIdx.Length - 1; ++i)
        {
            var idx1 = nearestBorderIdx[i];
            var idx2 = nearestBorderIdx[i + 1];
            var pos1 = Border[idx1];
            var pos2 = Border[idx2];
            if (Core.Utils.Intersect.IsIntersectLineWithLine(startPos, endPos, pos1, pos2, out Vector3 intersection))
            {
                return intersection;
            }
        }

        return startPos;
    }

    public bool IsWalkable(Vector3 pos)
    {
        if (Core.Utils.Math.IsInPolygon(Border, pos) == false)
        {
            return false;
        }

        foreach (var item in Blocks)
        {
            var dis = Vector3.Distance(pos, item.Center);
            if (dis > item.Length)
            {
                continue;
            }

            if (Core.Utils.Math.IsInPolygon(item.Points, pos))
            {
                return false;
            }
        }

        return true;
    }

    private void UpdatePosWhenOutsideBorder(ref Vector3 pos)
    {
        if (Core.Utils.Math.IsInPolygon(Border, pos) == false)
        {
            float minSqrDis2Border = float.PositiveInfinity;
            int minIdx = -1;
            for (int i = 0; i < Border.Length; ++i)
            {
                var p = Border[i];
                p.y = 0;
                Vector3 tmpDir = p - pos;
                if (tmpDir.sqrMagnitude < minSqrDis2Border)
                {
                    minSqrDis2Border = tmpDir.sqrMagnitude;
                    minIdx = i;
                }
            }

            if (minIdx != -1)
            {
                int secondMinIdx1 = (minIdx + 1) % Border.Length;
                float secondSqrDis1 = (Border[secondMinIdx1] - pos).IgnoreY().sqrMagnitude;
                int secondMinIdx2 = (minIdx - 1 + Border.Length) % Border.Length;
                float secondSqrDis2 = (Border[secondMinIdx2] - pos).IgnoreY().sqrMagnitude;
                Vector3 minLine;
                if (secondSqrDis1 < secondSqrDis2)
                {
                    minLine = Border[secondMinIdx1] - Border[minIdx];
                }
                else
                {
                    minLine = Border[minIdx] - Border[secondMinIdx2];
                }

                Vector3 normal = Vector3.Cross(minLine.IgnoreY(), Vector3.up);
                if (BorderOrient == BorderClockWise.ClockWise)
                {
                    normal = -normal;
                }

                float cos = Vector3.Dot(normal.normalized, (pos - Border[minIdx]).IgnoreY());
                pos = pos + normal.normalized * (Mathf.Abs(cos) + 1f);
            }
        }
    }

    public void SetSafeBoxData(GameObject goRoot)
    {
        GoSafeRoot = goRoot;
    }

    public void SetSafeBoxEnable(bool isActive)
    {
        if (null != GoSafeRoot)
        {
            GoSafeRoot.SetActive(isActive);
        }
    }
}
